/*
 * @Description: NDT匹配的实现
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:09:39
 * @LastEditTime: 2021-10-27 14:13:52
 * @LastEditors: Sang Hao
 */
#include "lidar_slam/models/registration/ndt_registration.hpp"

namespace lidar_slam {

NDTRegistration::NDTRegistration(const YAML::Node& node) 
	:ndt_ptr_(new pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>()) {
	float res = node["res"].as<float>();
	float step_size = node["step_size"].as<float>();
	float trans_eps = node["trans_eps"].as<float>();
	int max_iter = node["max_iter"].as<int>();
	SetRegistrationParam(res, step_size, trans_eps, max_iter);
}


NDTRegistration::NDTRegistration(float res, float step_size, float trans_eps, int max_iter) 
	:ndt_ptr_(new pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>()) {
	SetRegistrationParam(res, step_size, trans_eps, max_iter);
}

bool NDTRegistration::SetRegistrationParam(
	float res, float step_size, float trans_eps, int max_iter) {
	ndt_ptr_->setResolution(res);
	ndt_ptr_->setStepSize(step_size);
	ndt_ptr_->setTransformationEpsilon(trans_eps);
	ndt_ptr_->setMaximumIterations(max_iter);
	
	std::cout 	<< "NDT 的匹配参数为：" << std::endl
				<< "res: " << res << ", "
				<< "step_size: " << step_size << ", "
				<< "trans_eps: " << trans_eps << ", "
				<< "max_iter: " << max_iter 
				<< std::endl << std::endl;
	
	return true;

}


bool NDTRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) {
	ndt_ptr_->setInputTarget(input_target);
	return true;
}

bool NDTRegistration::ScanMatch(const CloudData::CLOUD_PTR& input_source, 
								const Eigen::Matrix4f& predict_pose,
								CloudData::CLOUD_PTR& result_cloud_ptr,
								Eigen::Matrix4f& result_pose) {
	ndt_ptr_->setInputSource(input_source);
	ndt_ptr_->align(*result_cloud_ptr, predict_pose);
	result_pose = ndt_ptr_->getFinalTransformation();
	return true;
} 

float NDTRegistration::GetFitnessScore() {
	return ndt_ptr_->getFitnessScore();
}
}